/********************************************
 * Field & Service Robotics Course 2011
 * Team Mars (Group 5)
 * Manipulator Class File
 * Tilman Pfieffer, Dorian Tsai, Roberto Sosa
 * 2011 November 6
 *
 * The manipulator class houses all of the functions required to tele-operate and control the
 * Mars Manipulator with the J2B2 robot.
 ********************************************
 */

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <math.h>
#include "manipulator.h"
//#include "serialstd.hpp"
#include "owndebug.h"
#include <assert.h>


using namespace std;

//#define USB_MICRO "/dev/ttyACM0"
#define TORAD 0.017453292
#define TODEG 57.29577951
#define PIhalf 1.570796326794897

//initialization
std::string USB_MICRO;


/* CONSTRUCTOR
 * manipulator class constructor
 * link lengths and joint angles are set. Positions are then automatically calculated using forward kinematics
 */
Manipulator::Manipulator(float s, float e, float w, float v, float g) {

    // joint/actuator positions
    // note that s,e,w are received in [rad] from the client computer
    // v is received in [mm] from the client computer
    S = s; // angle of shoulder joint [rad] in manipulator frame
    E = e; // angle of elbow joint [rad]
    W = w; // angle of wrist joint [rad] - currently not implemented - set to zero!
    V = v; // vertical position of the linear actuator [mm]

	Smove = S;
	Emove = E;
	Wmove = W;
	Vmove = V;
	Gmove = g;

    // end-effector pose
    float *pos=fwdKinematics(s,e,v,w);
    X = pos[0]; // x position [mm] left when in J2B2 position looking down and behind (double check - might have changed)
    Y = pos[1]; // y position [mm] right
    Z = pos[2]; // z position [mm] vertical
    C = pos[3]; // rad

    // gripper position
    G = g; // gripper position [deg] (since it is not used in mathematical calculations!)

    USB_MICRO="/dev/ttyACM0"; //usb port of microcontroller
    bool disp = true;
    updateState(disp);
}

// add current state to lists
void Manipulator::addStateToVector() {
	SV.push_back(S);
	EV.push_back(E);
	WV.push_back(W);
	VV.push_back(V);
	GV.push_back(G);
	XV.push_back(X);
	YV.push_back(Y);
	ZV.push_back(Z);
	CV.push_back(C);

	dPrint(1,"Show current vector S");


}
/* SET USB
 * sets the usb device (name) used on the computer, since Atmega microcontroller seems to change board names easily
 */
    void Manipulator::setUSB(std::string usb) {
  USB_MICRO=usb;
}

/* SET JOINTS SEV W + G
 * sets the joint / actuator angles and positions of the manipulator class and gripper
 * once limits are confirmed, the manipulator state (internal model) is updated and
 * commands are sent to the manipulator microcontroller
 */
void Manipulator::setJoints(float shoulder, float elbow, float vertical, float wrist) {
    // calculate would-be pose of manipulator using fwdKinematics
    float *pos=fwdKinematics(shoulder,elbow,vertical,wrist);
    bool disp = true;
    //check limits of would-be pose
    // if limits are ok, update the state of the manipulator by writing to writeMicroAction
    if (checkLimits(pos[0],pos[1], pos[2], pos[3], shoulder, elbow, vertical, wrist, G,disp)) {
        /*old case where state variables were overwritten
        // X,Y,Z and C are also set to ensure internal model matches real world
        X = pos[0];         Y = pos[1];        Z = pos[2];        C = pos[3];
        // updating state of manipulator
        S = shoulder;         E = elbow;        V = vertical;        W = wrist;
        G = gripper;
        */

        // having updated the state, corresponding commands are sent to the man. uC
        writeMicroAction(shoulder, elbow, vertical, wrist);
		// try reseting uC after each write of action to keep buffers clear and prevent servos from freezing up
		//writeMicroReset();
    }
    else{dPrint(1,"Commands exceed limits! Command NOT sent!");}
}

/* SET POSE XYZ C + G
 * sets the manipulator pose (position and orientation) and gripper
 */
void Manipulator::setPose(float xPos, float yPos, float zPos, float c) {

    //invKinematics(xPos,  yPos,  zPos , c);
	// c might change a bit due to algorithm
    float *anglesC=invKinematics(xPos, yPos, zPos , c);
    bool disp = true;

    // check limits of would-be position
    if(checkLimits(xPos, yPos, zPos, anglesC[4], anglesC[0], anglesC[1], anglesC[2], anglesC[3], G,disp)){
      /* old case were state variables were overwritten
       // recall that capital X etc are manipulator state variables
        X = xPos;         Y = yPos;        Z = zPos;        C = c;
        S = angles[0];         E = angles[1];        V = angles[2];        W = angles[3]; G = ...
*/
        // write positions to microcontroller
        writeMicroAction(anglesC[0],anglesC[1], anglesC[2], anglesC[3]);
		// try reseting uC after each write of action to keep buffers clear and prevent servos from freezing up
		//writeMicroReset();
    }
    else{dPrint(1,"Commands exceed limits! Command NOT sent!");}
}

void Manipulator::setGripper(float gripper){
    if(checkLimits(X, Y, Z, C, S, E, V, W, gripper,true)){
        writeMicroGripper(gripper);
    }
    else {
        dPrint(1,"Gripper command exceeds limit! Command NOT sent!");
    }
}
Manipulator::ESetResult Manipulator::SetElement(float position, int joint){
    float margin=2.0; //[mm]
    float anglemargin=2.0; // 2 degrees
    int gripAngleMargin = 5; // [deg] grip angle margin is larger due to analog errors in reading, already in degrees though!
    bool doPose = false;
    bool doJoints = false;
    bool doGripper = false;
    Manipulator::ESetResult changed = UNKNOWN;


    switch(joint) {

        case 0:{
                if ((position > X+margin) || (position < X-margin)) {
                    xS = position;
                    doPose = true;
                }
                break;
        }
        case 1:{
                if ((position > Y+margin) || (position < Y-margin)) {
                    doPose = true;
                    yS = position;
                }
                break;
        }
        case 2:{
                if ((position > Z+margin) || (position < Z-margin)) {
                    doPose = true;
                    zS = position;
                }
                break;
        }
        case 3:{
                if ((position > C*TODEG+anglemargin) || (position < C*TODEG-anglemargin)) {
                    doPose = true;
                    cS = position;
                }
                break;
        }
        case 4:{
                if (position < S*TODEG-anglemargin || position > S*TODEG+anglemargin) {
                    sS = position*TORAD;
                    doJoints=true;
                }
                break;
        }
        case 5:{
                if (position < E*TODEG-anglemargin || position > E*TODEG+anglemargin){
                    eS = position*TORAD;
                    doJoints=true;
                }
                break;
        }
        case 6:{
                if (position < V-margin || position > V+margin){
                    vS = position;
                    doJoints=true;
                }
                break;
        }
        case 7:{
                if (position < W*TODEG-anglemargin || position > W*TODEG+anglemargin) {
                    wS = position*TORAD;
                    doJoints=true;
                }
                break;
        }
        case 8:{
                if (position < G-gripAngleMargin || position > G+gripAngleMargin) {
                gS = position;
                doGripper = true;
                }
                break;
        }
        default:{
                dPrint(1,"UNKNOWN JOINT NUMBER");
                break;
        }

    }//end switch
    changed =   (doPose) ? POSE :
                (doJoints) ? JOINTS :
                (doGripper) ? GRIPPER : NONE;

    return changed;
    /*
    // if x,y,z,c are outside a certain margin => then accept the pose and try to set it (according to checkLimits)
    // PROBLEM WITH THIS IF - IF JUST ONE CONDITION IS OK THEN THE ENTIRE POSE IS ACCEPTED!
    if( x > X+margin || x < X-margin || y > Y+margin || y < Y-margin ||z > Z+margin || z < Z-margin ||c > C+margin || c < C-margin ){
        setPose(x,y,z,c);
	}
	else {
	    dPrint(1,"no change in pose x=%f y=%f z=%f c=%f ",X,Y,Z,C);
	    if (s < S-anglemargin ||s > S+anglemargin || e < E-anglemargin ||e > E+anglemargin || v > V+margin || v < V-margin || w < W-anglemargin ||w> W+anglemargin || g > G+margin || g < G-margin){
            setJoints(s,e,v,w,g);
            }
        else {
            dPrint(1,"no change in angle");
            }
	}*/

}

void Manipulator::doSet(Manipulator::ESetResult whatchanged) {

    switch(whatchanged) {

    case POSE:{
                setPose(xS,yS,zS,cS);
                dPrint(1,"change in pose x=%f y=%f z=%f c=%f ",X,Y,Z,C);
                stateChangeFlag = 1;
                break;
        }
    case JOINTS:{
                //dPrint(1,"wS = %f ",wS);
                setJoints(sS,eS,vS,wS);
                dPrint(1,"change in joints s=%f e=%f v=%f w=%f ",S,E,V,W);
                stateChangeFlag = 1;
                break;
        }
    case GRIPPER:{
                setGripper(gS);
                dPrint(1,"change in gripper g=%i ",G);
                stateChangeFlag = 1;
                break;
        }
    case NONE:{
                dPrint(1,"no changes at all");
                stateChangeFlag = 2;
                break;
        }
    case UNKNOWN:{
                dPrint(1,"UNKNOWN change");
                stateChangeFlag = 0;
                break;
        }
    }
}
/* SET new positions and angles
 * receives command from server and executes either position or angle changes
 * depending on what was put in
 */

/*
void Manipulator::Set(float x,float y,float z, float c,  float s, float e, float v, float w, float g){
    int margin=2.0; //[mm]
    float anglemargin=2.0*TORAD; // 2 degrees
    int gripAngleMargin = 5; // [deg] grip angle margin is larger due to analog errors in reading, already in degrees though!
    c*=TORAD;
    s*=TORAD;
    e*=TORAD;
    w*=TORAD;
    bool doPose = false;

    // default temp variables to state variables and then "upgrade" if there are any large changes (outside the margin)
    float xTemp = X; float yTemp = Y; float zTemp = Z; float cTemp = C;
    if ((x > X+margin) || (x < X-margin)) {
        xTemp = x;
        doPose = true;
    }
    if ((y > Y+margin) || (y < Y-margin)) {
        doPose = true;
        yTemp = y;
    }
    if ((z > Z+margin) || (z < Z-margin)) {
        doPose = true;
        zTemp = z;
    }
    if ((c > C+margin) || (c < C-margin)) {
        doPose = true;
        cTemp = c;
    }

    if (doPose) {
        setPose(xTemp,yTemp,zTemp,cTemp);
    } else {
        dPrint(1,"no change in pose x=%f y=%f z=%f c=%f ",X,Y,Z,C);

        float sTemp = S; float eTemp = E; float vTemp = V; float wTemp = W; int gTemp = G;
        bool doJoints = false;
        if (s < S-anglemargin || s > S+anglemargin) {
            sTemp = s;
            doJoints=true;
            }
        if (e < E-anglemargin || e > E+anglemargin){
            eTemp = e;
            doJoints=true;
            }
        if (w < W-anglemargin || w > W+anglemargin) {
            wTemp = w;
            doJoints=true;
            }
        if (v < V-margin || v > V+margin){
            vTemp = v;
            doJoints=true;
            }

        if (doJoints) setJoints(sTemp,eTemp,vTemp,wTemp);
            else{
            dPrint(1,"no change in  s=%f e=%f v=%f w=%f ",S,E,V,W);
        }
        if (g < G-gripAngleMargin || g > G+gripAngleMargin) {
            gTemp = g;
            setGripper(gTemp);
        }
    }

    // if x,y,z,c are outside a certain margin => then accept the pose and try to set it (according to checkLimits)
    // PROBLEM WITH THIS IF - IF JUST ONE CONDITION IS OK THEN THE ENTIRE POSE IS ACCEPTED!
    if( x > X+margin || x < X-margin || y > Y+margin || y < Y-margin ||z > Z+margin || z < Z-margin ||c > C+margin || c < C-margin ){
        setPose(x,y,z,c);
	}
	else {
	    dPrint(1,"no change in pose x=%f y=%f z=%f c=%f ",X,Y,Z,C);
	    if (s < S-anglemargin ||s > S+anglemargin || e < E-anglemargin ||e > E+anglemargin || v > V+margin || v < V-margin || w < W-anglemargin ||w> W+anglemargin || g > G+margin || g < G-margin){
            setJoints(s,e,v,w,g);
            }
        else {
            dPrint(1,"no change in angle");
            }
	}
}
*/
//get all
float *Manipulator::Get(){

    float *all = new float[9];
    all[0]=X;
    all[1]=Y;
    all[2]=Z;
    all[3]=C*TODEG;
    all[4]=S*TODEG;
    all[5]=E*TODEG;
    all[6]=V;
    all[7]=W*TODEG;
    all[8]=G;
    return all;
}
/*  GET JOINTS SEVW + G
 * returns the joint positions and vertical position as a ptr to an array
 */
float *Manipulator::getJoints() {

    float *ang = new float[5];
    ang[0] = S*TODEG;
    ang[1] = E*TODEG;
    ang[2] = V;
    ang[3] = W*TODEG;
    ang[4] = G;
    return ang;
}

/* GET POSE XYZC + G
 * returns the position of the end effector and its orientation as a ptr to an array
 */
float *Manipulator::getPose() {

    float *pose = new float[4];
    pose[0] = X;
    pose[1] = Y;
    pose[2] = Z;
    pose[3] = C*TODEG;
    return pose;
}

// simple get functions for individual access
float Manipulator::getX() {
    return X;
}

float Manipulator::getY() {

    return Y;
}
float Manipulator::getZ() {

    return Z;
}
float Manipulator::getC() {

    return C*TODEG;
}
float Manipulator::getS() {

    return S*TODEG;
}
float Manipulator::getE() {

    return E*TODEG;
}
float Manipulator::getW() {

    return W*TODEG;
}
float Manipulator::getV(){

    return V;
}
float Manipulator::getG(){

    return G; // already in degrees
}

/* CHECK LIMITS
 * check limits of the manipulator class to ensure everything is within the physical limits of operation
 * everything is within limits => returns okay = one, else okay = 0
 * TO DO: ACCOUNT FOR PHYSICAL LIMITS OF MANIPULATOR AND SHELF WALLS!
 */
int Manipulator::checkLimits(float x, float y, float z, float c, float s, float e, float v,float w, float g, bool disp) {

    int okay = 1; // set default for okay status (okay = 1 is "within limits")
    exceedLimitsFlag =0;
	// joint limits now found in manipulator class header as private variables

    // convert s and e to int type for better comparison
    s   = (int) (s*TODEG);
    e   = (int) (e*TODEG);
    w   = (int) (w*TODEG);

    // check for servo limits
    if ( (s < sMin)||(s > sMax) ) {
        if (disp)
            dPrint(1, "CHECK ERROR: s=%f exceeded",s);
        okay = 0;
        exceedLimitsFlag++;
    }
    if ( (e < eMin)||(e > eMax) ) {
        if (disp)
            dPrint(1, "CHECK ERROR: e=%f exceeded",e );
        okay = 0;
        exceedLimitsFlag++;
    }
    if ( (w < wMin)||(w > wMax) ) {
        if (disp)
            dPrint(1, "CHECK ERROR: w=%f exceeded",w);
        okay = 0;
        exceedLimitsFlag++;
    }
    if ( (v < vMin)||(v > vMax) ) {
        if (disp)
            dPrint(1, "CHECK ERROR: v=%f exceeded",v );
        okay = 0;
        exceedLimitsFlag++;
    }
    if (g < gMin || g > gMax) {
        if (disp)
            dPrint(1,"CHECK ERROR: g exceeded");
        okay = 0;
        exceedLimitsFlag++;
    }

    // manipulator limits
    float rMax = L02+ L1 + L2 + L3 + L4; // fully extended
    float rMin = sqrt(pow(((L3+L4)*cos(0.0+eMax+wMax)+L2*cos(0.0+eMax)+L1*cos(0.0)+L02),2) + pow(((L3+L4)*sin(0.0+eMax+wMax)+L2*sin(0.0+eMax)+L1*sin(0.0)),2)); // fully bent, which is no longer possible
    float zMax = 200.0; // fully retracted downwards to the ground[mm]
    float zMin = 0.0; // fully extended

	
	// manipulator limits
	float y_wrist = L2*sin(s+e) + L1*sin(s);				//y position of wrist joint
	float x_wrist = L2*cos(s+e) + (L1+L02)*cos(s);			//x position of wrist joint
	float J2B2_r = 260 + 40;									//Radius of J2B2 + some radius "protection" in mm
	float rMax = L02+ L1 + L2 + L3 + L4;					// fully extended
    float rMin = sqrt(pow(((L3+L4)*cos(0.0+eMax+wMax)+L2*cos(0.0+eMax)+L1*cos(0.0)+L02),2) + pow(((L3+L4)*sin(0.0+eMax+wMax)+L2*sin(0.0+eMax)+L1*sin(0.0)),2)); // fully bent, which is no longer possible
    float zMax = 200.0;										// fully retracted downwards to the ground[mm]
    float zMin = 0.0;										// fully extended
	float r_gripper = pow((x+300),2) + pow(y,2);				//distance from J2B2 center to gripper
	float r_wrist = pow((x_wrist+300),2) + pow(y_wrist,2);	//distance from J2B2 center to wrist joint
	
	
    // get current planar position for rMin/rMax comparison
    float planarPos = sqrt(x*x + y*y);

    // check for range limits
    if (planarPos > rMax) {
        if (disp)
            dPrint(1,"CHECK ERROR: planarPos > rMax, x = %f, y = %f,rMax = %f",x,y,rMax);
            okay = 0;
            exceedLimitsFlag++;
        }
        if (planarPos < rMin) {
            if (disp)
            dPrint(1,"CHECK ERROR: planarPos < rMin, x = %f, y = %f, rMin = %f",x,y,rMin);
            okay = 0;
            exceedLimitsFlag++;
        }
        if (z > zMax) {
            if (disp)
            dPrint(1,"CHECK ERROR: z > zMax");
            okay = 0;
            exceedLimitsFlag++;
        }
        if (z < zMin) {
            if (disp)
            dPrint(1,"CHECK ERROR: z < zMin");
            okay = 0;
            exceedLimitsFlag++;
        }

		if (r_gripper <= J2B2_r {
			//point is inside J2B2
			if (disp)
				dPrint(1,"CHECK ERROR: gripper position inside J2B2");
			okay = 0;
			exceedLimitsFlag++;
		}
		//Check limits for wrist joint
		if (r_wrist <= J2B2_r {
			//point is inside J2B2
			if (disp)
				dPrint(1,"CHECK ERROR: elbow joint position inside J2B2");
			okay = 0;
			exceedLimitsFlag++;
		}

		
    return okay;
}

/* FWD KINEMATICS
 * given joint positions, calculates the xyz,c positions (ie, pose)
 * based on basic SCARA robot configuration
 * coordinate system defined in final report (in progress)
 */
float *Manipulator::fwdKinematics (float s, float e, float v,float w) {
    float *tipPose = new float[4];

    //vector< vector<float> > T0te(4,vector<float>(4,0));
	float T0te[4][4];
    Manipulator::endTransform(T0te,s,e,v,w);

    // do calc for x,y,z and theta
    tipPose[0] = T0te[0][3]; // x
    tipPose[1] = T0te[1][3]; // y
    tipPose[2] = T0te[2][3]; // z
    tipPose[3] = s+e+w; //  c in rad

    return tipPose;
}

// calculates the end effector position and orientation and adjusts the values of T0e

//void Manipulator::endTransform(vector< vector<float> > T0te, float s, float e, float v, float w) {
void Manipulator::endTransform(float T0te[][4], float s, float e, float v, float w) {
    T0te[0][0] = cos(s+e+w);
    T0te[0][1] = sin(s+e+w);
    T0te[0][2] = 0;
    T0te[0][3] = (L3+L4)*cos(s+e+w) + L2*cos(s+e) + L1*cos(s) + L02;

    T0te[1][0] = sin(s+e+w);
    T0te[1][1] = -cos(s+e+w);
    T0te[1][2] = 0;
    T0te[1][3] = (L4+L3)*sin(s+e+w) + L2*sin(s+e) + L1*sin(s);

    T0te[2][0] = 0;
    T0te[2][1] = 0;
    T0te[2][2] = -1;
    T0te[2][3] = -v-L5+L01+Z0;

    T0te[3][0] = 0;
    T0te[3][1] = 0;
    T0te[3][2] = 0;
    T0te[3][3] = 1;
}


void Manipulator::Jacobian(float J[][4], float s, float e, float v, float w) {
    J[0][0] = -(L3+L4)*sin(s+e+w)-L2*sin(s+e)-L1*sin(s);
    J[0][1] = -(L3+L4)*sin(s+e+w) - L2*sin(s+e);
    J[0][2] = -(L3+L4)*sin(s+e+w);
    J[0][3] = 0;

    J[1][0] = (L3+L4)*cos(s+e+w) + L2*cos(s+e) + L1*cos(s);
    J[1][1] = (L3+L4)*cos(s+e+w) + L2*cos(s+e);
    J[1][2] = (L3+L4)*cos(s+e+w);
    J[1][3] = 0;

    J[2][0] = 0;
    J[2][1] = 0;
    J[2][2] = 0;
    J[2][3] = -1;

    J[3][0] = 1;
    J[3][1] = 1;
    J[3][2] = 1;
    J[3][3] = 0;
}

void Manipulator::JacobianNoOrient(float J[][3], float s, float e, float v, float w) {
    J[0][0] = -(L3+L4)*sin(s+e+w)-L2*sin(s+e)-L1*sin(s);
    J[0][1] = -(L3+L4)*sin(s+e+w) - L2*sin(s+e);
    J[0][2] = -(L3+L4)*sin(s+e+w);

    J[1][0] = (L3+L4)*cos(s+e+w) + L2*cos(s+e) + L1*cos(s);
    J[1][1] = (L3+L4)*cos(s+e+w) + L2*cos(s+e);
    J[1][2] = (L3+L4)*cos(s+e+w);

    J[2][0] = 0;
    J[2][1] = 0;
    J[2][2] = 0;

    J[3][0] = 1;
    J[3][1] = 1;
    J[3][2] = 1;
}

void Manipulator::WeightMatrix(float Wgt[][3], float s, float e, float v, float w, float sPast, float ePast, float vPast, float wPast) {

	float sMaxR = sMax*TORAD;
	float sMinR = sMin*TORAD;
	float eMaxR = eMax*TORAD;
	float eMinR = eMin*TORAD;
	float wMaxR = wMax*TORAD;
	float wMinR = wMin*TORAD;

	// define joint limits in class constant
    float h1 = (sMaxR-sMinR)*(sMaxR-sMinR)*(2.0*s-sMaxR-sMinR)/(4.0*(sMaxR-s)*(sMaxR-s)*(s-sMinR)*(s-sMinR));
    float h2 = (eMaxR-eMinR)*(eMaxR-eMinR)*(2.0*e-eMaxR-eMinR)/(4.0*(eMaxR-e)*(eMaxR-e)*(e-eMinR)*(e-eMinR));
    float h3 = (wMaxR-wMinR)*(wMaxR-wMinR)*(2.0*w-wMaxR-wMinR)/(4.0*(wMaxR-w)*(wMaxR-w)*(w-wMinR)*(w-wMinR));
	//float h4 = (vMax-vMin)*(vMax-vMin)*(2.0*v-vMax-vMin)/(4.0*(vMax-v)*(vMax-v)*(v-vMin)*(v-vMin));

	//dPrint(1,"h3 = %f",h3);

    float h1Past = (sMaxR-sMinR)*(sMaxR-sMinR)*(2.0*sPast-sMaxR-sMinR)/(4.0*(sMaxR-sPast)*(sMaxR-sPast)*(sPast-sMinR)*(sPast-sMinR));
    float h2Past = (eMaxR-eMinR)*(eMaxR-eMinR)*(2.0*ePast-eMaxR-eMinR)/(4.0*(eMaxR-ePast)*(eMaxR-ePast)*(ePast-eMinR)*(ePast-eMinR));
    float h3Past = (wMaxR-wMinR)*(wMaxR-wMinR)*(2.0*wPast-wMaxR-wMinR)/(4.0*(wMaxR-wPast)*(wMaxR-wPast)*(wPast-wMinR)*(wPast-wMinR));
	//float h4Past = (vMax-vMin)*(vMax-vMin)*(2.0*vPast-vMax-vMin)/(4.0*(vMax-vPast)*(vMax-vPast)*(vPast-vMin)*(vPast-vMin));

    float w1 = h1;
    float w2 = h2;
    float w3 = h3;
    //float w4 = h4;

    if (h1-h1Past >= 0)
        w1 = 1.0;
    if (h2-h2Past >= 0)
        w2 = 1.0;
    if (h3-h3Past >= 0)
        w3 = 1.0;
    //if (h4-h4Past >= 0)
     //   w4 = 1.0;

    Wgt[0][0] = w1;
    Wgt[0][1] = 0;
    Wgt[0][2] = 0;
    //Wgt[0][3] = 0;

    Wgt[1][0] = 0;
    Wgt[1][1] = w2;
    Wgt[1][2] = 0;
    //Wgt[1][3] = 0;

    Wgt[2][0] = 0;
    Wgt[2][1] = 0;
    Wgt[2][2] = w3;
    //Wgt[2][3] = 0;

	/*
    Wgt[3][0] = 1;
    Wgt[3][1] = 1;
    Wgt[3][2] = 1;
    Wgt[3][3] = w4;
	*/
	//dPrint(1,"w3 = %f",w3);
}

float Manipulator::fMod(float x, float y) {
	int temp = static_cast <int> (x/y);
	return (x - static_cast <double>(temp)*y);
}

// wrap angle about +/ 180
float Manipulator::wrapAngle(float c) {
	float cDeg = c*TODEG;
	float cWrap = cDeg;//%360;
	if (cDeg < 0.0) {
		cDeg = fMod(cDeg,-360.0);
		cWrap = 360.0 + cDeg;
	}
	else {
		cDeg = fmod(cDeg,360.0);
		cWrap = cDeg;
	}

	if (cWrap > 180.0) {
		cWrap = -(360.0 - cWrap);
	}
	return cWrap*TORAD;
}

float *Manipulator::dampedLeastSquaresNoOrient(float J[][3],float dX, float dY, float dZ) {
    float lambda = 0.01; // damping coefficient,

	float J11 = J[0][0];
	float J12 = J[0][1];
	float J13 = J[0][2];
	float J14 = J[0][3];
	
    float J21 = J[1][0];
	float J22 = J[1][1];
	float J23 = J[1][2];
	float J24 = J[1][3];
	
	float J31 = J[2][0];
	float J32 = J[2][1];
	float J33 = J[2][2];
	float J34 = J[2][3];
	
	

	//float lambda = 1.0;
	float lam2 = lambda*lambda;
	float DLS[4][3];

	DLS[0][0] = (-J13*J23*J21+J11*J23*J23+J11*lam2-J12*J22*J21+J11*J22*J22)/(lam2*lam2+(J22*J22+J23*J23+J21*J21+J11*J11+J13*J13+J12*J12)*lam2-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J11*J11*J23*J23+J11*J11*J22*J22+J12*J12*J23*J23-2.0*J11*J21*J12*J22+J13*J13*J22*J22+J13*J13*J21*J21-2.0*J12*J22*J13*J23);

	DLS[0][1] = -(J22*J12*J11-J21*lam2+J13*J23*J11-J21*J13*J13-J12*J12*J21)/(lam2*lam2+(J22*J22+J23*J23+J21*J21+J11*J11+J13*J13+J12*J12)*lam2-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J11*J11*J23*J23+J11*J11*J22*J22+J12*J12*J23*J23-2.0*J11*J21*J12*J22+J13*J13*J22*J22+J13*J13*J21*J21-2.0*J12*J22*J13*J23);
	
	DLS[0][2] = 0.0;
	
	DLS[1][0] = -(J13*J23*J22-J12*J21*J21+J11*J21*J22-J12*lam2-J12*J23*J23)/(lam2*lam2+(J22*J22+J23*J23+J21*J21+J11*J11+J13*J13+J12*J12)*lam2-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J11*J11*J23*J23+J11*J11*J22*J22+J12*J12*J23*J23-2.0*J11*J21*J12*J22+J13*J13*J22*J22+J13*J13*J21*J21-.0*J12*J22*J13*J23);
	
	DLS[1][1] = (J22*J11*J11+J22*lam2+J22*J13*J13-J11*J21*J12-J12*J13*J23)/(lam2*lam2+(J22*J22+J23*J23+J21*J21+J11*J11+J13*J13+J12*J12)*lam2-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J11*J11*J23*J23+J11*J11*J22*J22+J12*J12*J23*J23-2.0*J11*J21*J12*J22+J13*J13*J22*J22+J13*J13*J21*J21-2.0*J12*J22*J13*J23);
	
	DLS[1][2] = 0.0;
	
	DLS[2][0] = (-J12*J22*J23-J11*J21*J23+J13*lam2+J13*J22*J22+J13*J21*J21)/(lam2*lam2+(J22*J22+J23*J23+J21*J21+J11*J11+J13*J13+J12*J12)*lam2-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J11*J11*J23*J23+J11*J11*J22*J22+J12*J12*J23*J23-2.0*J11*J21*J12*J22+J13*J13*J22*J22+J13*J13*J21*J21-2.0*J12*J22*J13*J23);
	
	DLS[2][1] = -(J22*J12*J13-J23*lam2+J11*J21*J13-J23*J11*J11-J12*J12*J23)/(lam2*lam2+(J22*J22+J23*J23+J21*J21+J11*J11+J13*J13+J12*J12)*lam2-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J11*J11*J23*J23+J11*J11*J22*J22+J12*J12*J23*J23-2.0*J11*J21*J12*J22+J13*J13*J22*J22+J13*J13*J21*J21-2.0*J12*J22*J13*J23);
	
	DLS[2][2] = 0.0;
	
	DLS[3][0] = 0.0;
	
	DLS[3][1] = 0.0;
	
	DLS[3][2] =  -1.0/(1.0+lam2);
	
    float *dAngles = new float[4];

	// note that the order is NOT!!! adjusted to the "sevw" format
    dAngles[0] = DLS[0][0]*dX + DLS[0][1]*dY + DLS[0][2]*dZ; // S
    dAngles[1] = DLS[1][0]*dX + DLS[1][1]*dY + DLS[1][2]*dZ; // E
	
	dAngles[2] = DLS[2][0]*dX + DLS[2][1]*dY + DLS[2][2]*dZ; // W
	
	dAngles[3] = DLS[3][0]*dX + DLS[3][1]*dY + DLS[3][2]*dZ; // V
    return dAngles;
}

float *Manipulator::dampedLeastSquares(float J[][4],float dX, float dY, float dZ, float dC) {
    float lambda = 0.01; // damping coefficient,

	float J11 = J[0][0];
	float J12 = J[0][1];
	float J13 = J[0][2];
	float J14 = J[0][3];
	
    float J21 = J[1][0];
	float J22 = J[1][1];
	float J23 = J[1][2];
	float J24 = J[1][3];
	
	float J31 = J[2][0];
	float J32 = J[2][1];
	float J33 = J[2][2];
	float J34 = J[2][3];
	
	float J41 = J[3][0];
	float J42 = J[3][1];
	float J43 = J[3][2];
	float J44 = J[3][3];
	

	//float lambda = 1.0;
	float lam2 = lambda*lambda;
	float DLS[4][4];

	DLS[0][0] = (J11*lam2*lam2+(J11*J22*J22+2.0*J11+J11*J23*J23-J12*J22*J21-J13*J23*J21-J12-J13)*lam2+J11*J22*J22-J13*J22*J22+J21*J23*J12-J12*J23*J23-J12*J22*J21+J12*J22*J23+J13*J23*J22+J21*J22*J13+J11*J23*J23-2.0*J11*J22*J23-J13*J23*J21)/(lam2*lam2*lam2+(3.0+J11*J11+J21*J21+J23*J23+J13*J13+J12*J12+J22*J22)*lam2*lam2+(-2.0*J11*J13+2.0*J13*J13+2.0*J21*J21+J11*J11*J23*J23-2.0*J21*J23-2.0*J11*J21*J13*J23-2.0*J11*J21*J12*J22+J12*J12*J23*J23+2.0*J12*J12+J11*J11*J22*J22+2.0*J22*J22-2.0*J22*J23+J13*J13*J22*J22+J13*J13*J21*J21+J12*J12*J21*J21+2.0*J23*J23-2.0*J21*J22-2.0*J11*J12-2.0*J13*J12-2.0*J12*J22*J13*J23+2.0*J11*J11)*lam2-2.0*J21*J21*J12*J13+J13*J13*J22*J22-2.0*J12*J22*J13*J23+2.0*J22*J23*J11*J12-2.0*J11*J21*J12*J22-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J12*J12*J23*J23+2.0*J22*J23*J11*J13+J13*J13*J21*J21-2.0*J21*J22*J13*J13+2.0*J21*J22*J11*J13+J11*J11*J23*J23+J11*J11*J22*J22+2.0*J21*J22*J12*J13-2.0*J22*J22*J11*J13+2.0*J21*J23*J11*J12+2.0*J21*J23*J12*J13-2.0*J22*J23*J11*J11-2.0*J21*J23*J12*J12-2.0*J23*J23*J11*J12);
	
	DLS[0][1] = (J21*lam2*lam2+(-J13*J23*J11+J21*J13*J13-J22*J12*J11+J12*J12*J21+2.0*J21-J23-J22)*lam2+J23*J11*J12+J12*J13*J23-J13*J23*J11-J22*J12*J11-J12*J12*J23-J22*J13*J13+J12*J12*J21+J21*J13*J13+J22*J12*J13+J22*J11*J13-2.0*J21*J12*J13)/(lam2*lam2*lam2+(3.0+J11*J11+J21*J21+J23*J23+J13*J13+J12*J12+J22*J22)*lam2*lam2+(-2.0*J11*J13+2.0*J13*J13+2.0*J21*J21+J11*J11*J23*J23-2.0*J21*J23-2.0*J11*J21*J13*J23-2.0*J11*J21*J12*J22+J12*J12*J23*J23+2.0*J12*J12+J11*J11*J22*J22+2.0*J22*J22-2.0*J22*J23+J13*J13*J22*J22+J13*J13*J21*J21+J12*J12*J21*J21+2.0*J23*J23-2.0*J21*J22-2.0*J11*J12-2.0*J13*J12-2.0*J12*J22*J13*J23+2.0*J11*J11)*lam2-2.0*J21*J21*J12*J13+J13*J13*J22*J22-2.0*J12*J22*J13*J23+2.0*J22*J23*J11*J12-2.0*J11*J21*J12*J22-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J12*J12*J23*J23+2.0*J22*J23*J11*J13+J13*J13*J21*J21-2.0*J21*J22*J13*J13+2.0*J21*J22*J11*J13+J11*J11*J23*J23+J11*J11*J22*J22+2.0*J21*J22*J12*J13-2.0*J22*J22*J11*J13+2.0*J21*J23*J11*J12+2.0*J21*J23*J12*J13-2.0*J22*J23*J11*J11-2.0*J21*J23*J12*J12-2.0*J23*J23*J11*J12);
	
	DLS[0][2] = 0;
	
	DLS[0][3] = -(-lam2*lam2+(-J22*J22+J21*J23-J13*J13-J12*J12+J11*J13-J23*J23+J21*J22+J11*J12)*lam2+2.0*J12*J22*J13*J23-J12*J12*J23*J23-J22*J23*J11*J13+J22*J22*J11*J13-J21*J23*J12*J13-J22*J23*J11*J12+J23*J23*J11*J12+J21*J23*J12*J12+J21*J22*J13*J13-J13*J13*J22*J22-J21*J22*J12*J13)/(lam2*lam2*lam2+(3.0+J11*J11+J21*J21+J23*J23+J13*J13+J12*J12+J22*J22)*lam2*lam2+(-2.0*J11*J13+2.0*J13*J13+2.0*J21*J21+J11*J11*J23*J23-2.0*J21*J23-2.0*J11*J21*J13*J23-2.0*J11*J21*J12*J22+J12*J12*J23*J23+2.0*J12*J12+J11*J11*J22*J22+2*J22*J22-2.0*J22*J23+J13*J13*J22*J22+J13*J13*J21*J21+J12*J12*J21*J21+2.0*J23*J23-2.0*J21*J22-2.0*J11*J12-2.0*J13*J12-2.0*J12*J22*J13*J23+2.0*J11*J11)*lam2-2.0*J21*J21*J12*J13+J13*J13*J22*J22-2.0*J12*J22*J13*J23+2.0*J22*J23*J11*J12-2.0*J11*J21*J12*J22-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J12*J12*J23*J23+2.0*J22*J23*J11*J13+J13*J13*J21*J21-2.0*J21*J22*J13*J13+2.0*J21*J22*J11*J13+J11*J11*J23*J23+J11*J11*J22*J22+2.0*J21*J22*J12*J13-2.0*J22*J22*J11*J13+2.0*J21*J23*J11*J12+2.0*J21*J23*J12*J13-2.0*J22*J23*J11*J11-2.0*J21*J23*J12*J12-2.0*J23*J23*J11*J12);
	
	DLS[1][0] = -(-J12*lam2*lam2+(J13+J13*J23*J22-J12*J21*J21+J11*J21*J22-2.0*J12+J11-J12*J23*J23)*lam2-J11*J21*J23+2.0*J21*J23*J12-J12*J23*J23-J11*J22*J23-J12*J21*J21+J13*J21*J21-J13*J23*J21+J13*J23*J22-J21*J22*J13+J11*J21*J22+J11*J23*J23)/(lam2*lam2*lam2+(3.0+J11*J11+J21*J21+J23*J23+J13*J13+J12*J12+J22*J22)*lam2*lam2+(-2.0*J11*J13+2.0*J13*J13+2.0*J21*J21+J11*J11*J23*J23-2.0*J21*J23-2.0*J11*J21*J13*J23-2.0*J11*J21*J12*J22+J12*J12*J23*J23+2.0*J12*J12+J11*J11*J22*J22+2.0*J22*J22-2.0*J22*J23+J13*J13*J22*J22+J13*J13*J21*J21+J12*J12*J21*J21+2.0*J23*J23-2.0*J21*J22-2.0*J11*J12-2.0*J13*J12-2.0*J12*J22*J13*J23+2.0*J11*J11)*lam2-2.0*J21*J21*J12*J13+J13*J13*J22*J22-2.0*J12*J22*J13*J23+2.0*J22*J23*J11*J12-2.0*J11*J21*J12*J22-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J12*J12*J23*J23+2.0*J22*J23*J11*J13+J13*J13*J21*J21-2.0*J21*J22*J13*J13+2.0*J21*J22*J11*J13+J11*J11*J23*J23+J11*J11*J22*J22+2.0*J21*J22*J12*J13-2.0*J22*J22*J11*J13+2.0*J21*J23*J11*J12+2.0*J21*J23*J12*J13-2.0*J22*J23*J11*J11-2.0*J21*J23*J12*J12-2.0*J23*J23*J11*J12);
	
	DLS[1][1] = (J22*lam2*lam2+(J22*J13*J13-J11*J21*J12-J12*J13*J23+J22*J11*J11+2.0*J22-J23-J21)*lam2-J21*J13*J13+J23*J11*J12-J12*J13*J23+J13*J23*J11-J23*J11*J11+J22*J13*J13-2.0*J22*J11*J13+J11*J21*J13+J22*J11*J11-J11*J21*J12+J21*J12*J13)/(lam2*lam2*lam2+(3.0+J11*J11+J21*J21+J23*J23+J13*J13+J12*J12+J22*J22)*lam2*lam2+(-2.0*J11*J13+2.0*J13*J13+2.0*J21*J21+J11*J11*J23*J23-2.0*J21*J23-2.0*J11*J21*J13*J23-2.0*J11*J21*J12*J22+J12*J12*J23*J23+2.0*J12*J12+J11*J11*J22*J22+2.0*J22*J22-2.0*J22*J23+J13*J13*J22*J22+J13*J13*J21*J21+J12*J12*J21*J21+2.0*J23*J23-2.0*J21*J22-2.0*J11*J12-2.0*J13*J12-2.0*J12*J22*J13*J23+2.0*J11*J11)*lam2-2.0*J21*J21*J12*J13+J13*J13*J22*J22-2.0*J12*J22*J13*J23+2.0*J22*J23*J11*J12-2.0*J11*J21*J12*J22-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J12*J12*J23*J23+2.0*J22*J23*J11*J13+J13*J13*J21*J21-2.0*J21*J22*J13*J13+2.0*J21*J22*J11*J13+J11*J11*J23*J23+J11*J11*J22*J22+2.0*J21*J22*J12*J13-2.0*J22*J22*J11*J13+2.0*J21*J23*J11*J12+2.0*J21*J23*J12*J13-2.0*J22*J23*J11*J11-2.0*J21*J23*J12*J12-2.0*J23*J23*J11*J12);
	
	DLS[1][2] = 0;
	
	DLS[1][3] = -(-lam2*lam2+(-J23*J23-J11*J11+J11*J12-J13*J13+J21*J22-J21*J21+J22*J23+J13*J12)*lam2-J21*J23*J12*J13-J13*J13*J21*J21+J21*J22*J13*J13+2.0*J11*J21*J13*J23-J11*J11*J23*J23-J22*J23*J11*J13+J21*J21*J12*J13+J22*J23*J11*J11-J21*J23*J11*J12+J23*J23*J11*J12-J21*J22*J11*J13)/(lam2*lam2*lam2+(3.0+J11*J11+J21*J21+J23*J23+J13*J13+J12*J12+J22*J22)*lam2*lam2+(-2.0*J11*J13+2.0*J13*J13+2.0*J21*J21+J11*J11*J23*J23-2.0*J21*J23-2.0*J11*J21*J13*J23-2.0*J11*J21*J12*J22+J12*J12*J23*J23+2.0*J12*J12+J11*J11*J22*J22+2.0*J22*J22-2.0*J22*J23+J13*J13*J22*J22+J13*J13*J21*J21+J12*J12*J21*J21+2.0*J23*J23-2.0*J21*J22-2.0*J11*J12-2.0*J13*J12-2.0*J12*J22*J13*J23+2.0*J11*J11)*lam2-2.0*J21*J21*J12*J13+J13*J13*J22*J22-2.0*J12*J22*J13*J23+2.0*J22*J23*J11*J12-2.0*J11*J21*J12*J22-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J12*J12*J23*J23+2.0*J22*J23*J11*J13+J13*J13*J21*J21-2.0*J21*J22*J13*J13+2.0*J21*J22*J11*J13+J11*J11*J23*J23+J11*J11*J22*J22+2.0*J21*J22*J12*J13-2.0*J22*J22*J11*J13+2.0*J21*J23*J11*J12+2.0*J21*J23*J12*J13-2.0*J22*J23*J11*J11-2.0*J21*J23*J12*J12-2.0*J23*J23*J11*J12);

	DLS[2][0] = -(-J13*lam2*lam2+(-J13*J21*J21-J13*J22*J22+J12*J22*J23+J11*J21*J23+J12-2.0*J13+J11)*lam2-J11*J22*J23-J12*J22*J21-J13*J21*J21+J12*J22*J23-J13*J22*J22+J12*J21*J21-J21*J23*J12+J11*J21*J23+2.0*J21*J22*J13+J11*J22*J22-J11*J21*J22)/(lam2*lam2*lam2+(3.0+J11*J11+J21*J21+J23*J23+J13*J13+J12*J12+J22*J22)*lam2*lam2+(-2.0*J11*J13+2.0*J13*J13+2.0*J21*J21+J11*J11*J23*J23-2.0*J21*J23-2.0*J11*J21*J13*J23-2.0*J11*J21*J12*J22+J12*J12*J23*J23+2.0*J12*J12+J11*J11*J22*J22+2.0*J22*J22-2.0*J22*J23+J13*J13*J22*J22+J13*J13*J21*J21+J12*J12*J21*J21+2*J23*J23-2.0*J21*J22-2.0*J11*J12-2.0*J13*J12-2.0*J12*J22*J13*J23+2.0*J11*J11)*lam2-2.0*J21*J21*J12*J13+J13*J13*J22*J22-2.0*J12*J22*J13*J23+2.0*J22*J23*J11*J12-2.0*J11*J21*J12*J22-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J12*J12*J23*J23+2.0*J22*J23*J11*J13+J13*J13*J21*J21-2.0*J21*J22*J13*J13+2.0*J21*J22*J11*J13+J11*J11*J23*J23+J11*J11*J22*J22+2.0*J21*J22*J12*J13-2.0*J22*J22*J11*J13+2.0*J21*J23*J11*J12+2.0*J21*J23*J12*J13-2.0*J22*J23*J11*J11-2.0*J21*J23*J12*J12-2.0*J23*J23*J11*J12);

	DLS[2][1] = -(-J23*lam2*lam2+(J22-J23*J11*J11-J12*J12*J23-2.0*J23+J11*J21*J13+J21+J22*J12*J13)*lam2+J22*J11*J11+J22*J12*J13-J22*J11*J13-J22*J12*J11-J23*J11*J11-J12*J12*J23+J11*J21*J13+2.0*J23*J11*J12-J11*J21*J12+J12*J12*J21-J21*J12*J13)/(lam2*lam2*lam2+(3.0+J11*J11+J21*J21+J23*J23+J13*J13+J12*J12+J22*J22)*lam2*lam2+(-2.0*J11*J13+2.0*J13*J13+2.0*J21*J21+J11*J11*J23*J23-2.0*J21*J23-2.0*J11*J21*J13*J23-2.0*J11*J21*J12*J22+J12*J12*J23*J23+2.0*J12*J12+J11*J11*J22*J22+2.0*J22*J22-2.0*J22*J23+J13*J13*J22*J22+J13*J13*J21*J21+J12*J12*J21*J21+2.0*J23*J23-2.0*J21*J22-2.0*J11*J12-2.0*J13*J12-2.0*J12*J22*J13*J23+2.0*J11*J11)*lam2-2.0*J21*J21*J12*J13+J13*J13*J22*J22-2.0*J12*J22*J13*J23+2.0*J22*J23*J11*J12-2.0*J11*J21*J12*J22-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J12*J12*J23*J23+2.0*J22*J23*J11*J13+J13*J13*J21*J21-2.0*J21*J22*J13*J13+2.0*J21*J22*J11*J13+J11*J11*J23*J23+J11*J11*J22*J22+2.0*J21*J22*J12*J13-2.0*J22*J22*J11*J13+2.0*J21*J23*J11*J12+2.0*J21*J23*J12*J13-2.0*J22*J23*J11*J11-2.0*J21*J23*J12*J12-2.0*J23*J23*J11*J12);
	
	DLS[2][2] = 0;
	
	DLS[2][3] = (lam2*lam2+(J21*J21-J11*J13-J22*J23+J11*J11-J13*J12+J22*J22+J12*J12-J21*J23)*lam2+J21*J22*J12*J13+J22*J23*J11*J12+J11*J11*J22*J22-J21*J21*J12*J13-J22*J23*J11*J11-2.0*J11*J21*J12*J22+J12*J12*J21*J21-J22*J22*J11*J13-J21*J23*J12*J12+J21*J22*J11*J13+J21*J23*J11*J12)/(lam2*lam2*lam2+(3.0+J11*J11+J21*J21+J23*J23+J13*J13+J12*J12+J22*J22)*lam2*lam2+(-2.0*J11*J13+2.0*J13*J13+2.0*J21*J21+J11*J11*J23*J23-2.0*J21*J23-2.0*J11*J21*J13*J23-2.0*J11*J21*J12*J22+J12*J12*J23*J23+2.0*J12*J12+J11*J11*J22*J22+2.0*J22*J22-2.0*J22*J23+J13*J13*J22*J22+J13*J13*J21*J21+J12*J12*J21*J21+2.0*J23*J23-2.0*J21*J22-2.0*J11*J12-2.0*J13*J12-2.0*J12*J22*J13*J23+2.0*J11*J11)*lam2-2.0*J21*J21*J12*J13+J13*J13*J22*J22-2.0*J12*J22*J13*J23+2.0*J22*J23*J11*J12-2.0*J11*J21*J12*J22-2.0*J11*J21*J13*J23+J12*J12*J21*J21+J12*J12*J23*J23+2.0*J22*J23*J11*J13+J13*J13*J21*J21-2.0*J21*J22*J13*J13+2.0*J21*J22*J11*J13+J11*J11*J23*J23+J11*J11*J22*J22+2.0*J21*J22*J12*J13-2.0*J22*J22*J11*J13+2.0*J21*J23*J11*J12+2.0*J21*J23*J12*J13-2.0*J22*J23*J11*J11-2.0*J21*J23*J12*J12-2.0*J23*J23*J11*J12);
	
	DLS[3][0] = 0;

	DLS[3][1] = 0;
	
	DLS[3][2] = -1.0/(1.0+lam2);
	
	DLS[3][3] = 0;

	
    float *dAngles = new float[4];

	// note that the order is NOT!!! adjusted to the "sevw" format
    dAngles[0] = DLS[0][0]*dX + DLS[0][1]*dY + DLS[0][2]*dZ; // S
    dAngles[1] = DLS[1][0]*dX + DLS[1][1]*dY + DLS[1][2]*dZ; // E
	
	dAngles[2] = DLS[2][0]*dX + DLS[2][1]*dY + DLS[2][2]*dZ; // W
	
	dAngles[3] = DLS[3][0]*dX + DLS[3][1]*dY + DLS[3][2]*dZ; // V
    return dAngles;
}



// algebraic solution for inverse Kinematics, used as a component in the normal inverse Kinematics function
float *Manipulator::invKinematicsAlgebraic (float x, float y, float z, float c) {
    float *angles= new float[4];

    // system of eqns to solve:
    // x = L02 + L1*cos(s) + L2*cos(s+e) + (L3+L4)*cos(s+e+w) [1]
    // y = L1*sin(s) + L2*sin(s+e) + (L3+L4)*sin(s+e+w); [2]
    // z = -v-L5+L01+z0; [3]
    // c = s+e+w; [4]
    // decoupled vertical actuator

    float v = -z-L5+L01+Z0;

    // can rewrite [1] and [2]:
    // D = A*cos(s) + B*cos(s+e)
    // H = A*sin(s) + B*sin(s+e)
    // D,A,B,H are constants
    float D = x - L02 - (L3+L4)*cos(c);
    float A = L1;
    float B = L2;
    float H = y - (L3+L4)*sin(c);

    float F = (D*D + H*H - A*A - B*B)/(2.0*A);
	float e = 2.0*atan2(sqrt(B*B-F*F),(B+F)); // consider left-hand model

    if (y < 0) {
		e = 2.0*atan2(-sqrt(B*B-F*F),(B+F)); // consider right hand model
	}


    float M = A+B*cos(e);
    float N = B*sin(e);

    float P = (D*M+H*N)/(M*M+N*N);
    float R = (H-N*P)/M;

    float s = atan2(R,P);
    float w = c-s-e;

    angles[0] = wrapAngle(s);
    angles[1] = wrapAngle(e);
    angles[2] = v;
    angles[3] = wrapAngle(w);

    return angles;
}

// finds the minimum value of an array and returns minIndex of the array
int Manipulator::findMinArray(float array[], int size) {
	float minValue = array[0]; // initialize
	int minIndex = 0;

	for (int i = 0; i < size; i++) {
		if (array[i] < minValue) {
			minValue = array[i];
			minIndex = i;
		}
	}
	return minIndex;
}

// find min of list
int Manipulator::findMinVector(vector<float> L) {
    float minValue = L.front();
    int minIndex = 0;
    unsigned int i;
    //vector<int>::iterator it;

    //for (it = L.begin(); it < L.end(); it++) {
    for ( i = 0; i < L.size(); i++) {
        if (L.at(i) < minValue ) {
            minValue = L.at(i);
            minIndex = i;
        }
    }
    return minIndex;
}

/* INVERSE KINEMATICS
 * given the xyz,c pose, this function calculates the right-handed version of the joint positions
 */
 

float *Manipulator::invKinematics (float xGoal, float yGoal, float zGoal, float cGoal) {

	//dPrint(1,"Invoking invKinematics");
    dPrint(1,"\nxG = %f\nyG = %f\ncG = %f\n",xGoal,yGoal,cGoal*TODEG);
	float *angles= new float[4]; // ptr to store accepted angles
	float *anglesC= new float[5]; // ptr to store accepted angles + new orientation
    bool disp = false;

	///////////////////////////// Heuristic Orientation ///////////////////////////////
	float *anglesTry = new float[4]; // ptr to store temporary angles from invKinematicsAlgebraic
	int configSize = 37; // number of configurations taken into consideration for changing orientation
	float nRange = configSize-1.0; // same as configSize, but in float
	float delCRange = 180; // +- angle [deg] range between each change in orientation
	float *delC = new float[configSize];  // ptr to store change in orientation
	float *delConfig = new float[4]; // difference between current joint positions and attempted joint positions
	//float *delConfigCost = new float[configSize]; // ptr to store the results of the cost function for all the proposed configuration changes

    vector<float> delConfigCost; //vector to hold the cost function results
    vector<int> indDelC; // vector to hold indices of delC, excluding ones that failed checkLimits
	vector<float> delCvect;
    indDelC.push_back(0); // default 0 position to prevent seg fault


	// the idea is to loop from 0 to 360 deg for a change in delC
	// for each change, delC, try to find a set of s,e,w,v that are within the limits of the manipulator
	// and minimize the change in configuration (so the manipulator doesn't have to move much)
	for (int i = 0; i < configSize; i++) {
		delC[i] =  ((delCRange*2.0/nRange)*i - delCRange)*TORAD; // define change in orientation

		dPrint(1,"delC[%i] = %f",i,delC[i]*TODEG);

		anglesTry = invKinematicsAlgebraic(xGoal, yGoal, zGoal , cGoal + delC[i]); // calc. new set of s,e,v,w
		dPrint(1,"st = %f, et = %f, wt = %f",anglesTry[0]*TODEG,anglesTry[1]*TODEG,anglesTry[3]*TODEG);

		// check limits to make sure angles are valid
		// if not, not sure what to do...
		if(checkLimits(xGoal, yGoal, zGoal, cGoal+delC[i], anglesTry[0], anglesTry[1], anglesTry[2], anglesTry[3], G,disp)){
			delConfig[0] = S - anglesTry[0];
			delConfig[1] = E - anglesTry[1];
			delConfig[2] = V - anglesTry[2];
			delConfig[3] = W - anglesTry[3];

			//delConfigCost[i] = delConfig[0]*delConfig[0] + delConfig[1]*delConfig[1] + delConfig[3]*delConfig[3];
            float *pos=fwdKinematics(anglesTry[0],anglesTry[1],anglesTry[2],anglesTry[3]);
            float delAngleDot = (delConfig[0]*delConfig[0] + delConfig[1]*delConfig[1] + delConfig[3]*delConfig[3]);
            float posError = (xGoal-pos[0])*(xGoal-pos[0]) + (yGoal-pos[1])*(yGoal-pos[1]);
            // comparison of posError [mm] and delAngleDot [rad] is not good...
            delConfigCost.push_back(posError + delAngleDot); // min weighting => posError!
            delCvect.push_back(delC[i]);
			
            dPrint(1,"delConfigCost[%i] = %f",i,delConfigCost.back());
		}
		else {

		    delConfigCost.push_back(999999.0); // some temporary arbitrarily large number
            indDelC.push_back(i);
			delCvect.push_back(delC[i]);
			
			// want to reject s,e,w if checkLimits fail with the anglesTry
			// assign arbitrarily high values to anglesTry?
			/*
			delConfig[0] = S + 999.9;
			delConfig[1] = E + 999.9;
			delConfig[2] = V + 999.9;
			delConfig[3] = W + 999.9;*/
		}

		// with this done, can store each delConfig for corresponding delC or...
		// now, we immediately calculate the cost function to choose the best delC (the one with minimum change in configuration change)
		// only for s,e,w since v is decoupled and should not be an issue
		// our cost function is simply the dot product of the relevant angles
	}

	// find minimum configuration
	//int indMinConfigCost = findMinArray(delConfigCost,configSize); // not sure if delConfig can be passed properly, but should find indMinConfigCost

	// sort list from lower to higher and pop the top
	dPrint(1,"Entire delConfigCost vector:");
	for (unsigned int ii = 0; ii < delConfigCost.size(); ii++) {
	    dPrint(1,"Element: %f",delConfigCost.at(ii));
	}
	int indMinConfigCost = findMinVector(delConfigCost);
    dPrint(1,"delCongfigCost front: %f",delConfigCost.front());
    dPrint(1,"delCongfigCost back: %f",delConfigCost.back());
    dPrint(1,"delConfigCost iMin: %f",delConfigCost.at(indMinConfigCost));
	dPrint(1,"delCvect delC min: %f",delCvect.at(indMinConfigCost));
	dPrint(1,"delC number i: %i", indDelC.at(indMinConfigCost));
	//dPrint(1,"indMinConfigCost = %i", indMinConfigCost);

	//float minConfigCost = delConfigCost[indMinConfigCost];
	//dPrint(1,"minConfigCost = %f",minConfigCost);

	float cNew = cGoal + delC[indDelC.at(indMinConfigCost)]; // define new orientation

	angles = invKinematicsAlgebraic(xGoal, yGoal, zGoal , cNew); // apply to inverseKinematicsAlgebraic solution to find new set of angles


	/*
	///////////////////////////// Damped Least Squares ///////////////////////////////
	dPrint(1,"\n\nTest dampedLeastSquares \n");
	float wP = W_OLD;
	float vP = V_OLD;
	float sP = S_OLD;
	float eP = E_OLD;
	float delX = xGoal - X;
	float delY = yGoal - Y;
	float delZ = zGoal - Z;
	float *delAngles = dampedLeastSquaresLimited(delX, delY, delZ, sP,eP, vP, wP);
	angles[0] = S + delAngles[0];
	angles[1] = E + delAngles[1];
	angles[2] = V - delZ;
	angles[3] = W + delAngles[2];
	// update orientation
	float cNew = angles[0] + angles[1] + angles[3];

	//dPrint(1,"ANGLES\n");
	//dPrint(1,"old s = %f\te = %f\tw = %f\t",S*TODEG,E*TODEG,W*TODEG);
	//dPrint(1,"del s = %f\te = %f\tw = %f\t",delAngles[0]*TODEG,delAngles[1]*TODEG,delAngles[2]*TODEG);
	//dPrint(1,"new s = %f\te = %f\tw = %f\t",angles[0]*TODEG,angles[1]*TODEG,angles[3]*TODEG);


	*/
	float *posCurrent=fwdKinematics(S,E,V,W);
	float *posSend=fwdKinematics(angles[0],angles[1],angles[2],angles[3]);

	dPrint(1,"POSE\n");
	dPrint(1,"curr x = %f\ty = %f\tz = %fc = %f",posCurrent[0],posCurrent[1],posCurrent[2],posCurrent[3]);
	dPrint(1,"goal x = %f\ty = %f\tz = %fc = %f",xGoal,yGoal,zGoal,cGoal);
	dPrint(1,"send x = %f\ty = %f\tz = %fc = %f",posSend[0],posSend[1],posSend[2],posSend[3]);
	dPrint(1,"del x = %f\ty = %f\tz = %fc = %f",xGoal-posSend[0],yGoal-posSend[1],zGoal-posSend[2],cGoal-posSend[3]);

	anglesC[0] = angles[0];
    anglesC[1] = angles[1];
    anglesC[2] = angles[2];
    anglesC[3] = angles[3];
	anglesC[4] = cNew; // also return new C

    return anglesC;
}

/*
float *Manipulator::invKinematics (float xGoal, float yGoal, float zGoal, float cGoal) {
	
	float *angles= new float[4]; // ptr to store accepted angles
	
	
	// linear interpolate from the current pose to the goal pos
	// initialize loop 
	// get transformations for each point
	// 
	
	
}
*/

/* SEND JOINT INSTRUCTIONS TO MICROCONTROLLER
 * writes joint positions to microcontroller via serial communication from MaCI library
 * uses specific format: $As###e###v###g###;
 * $ specifies the start of a new message
 * A specifies actuator control
 * --
 * s specifies shoulder servo
 * ### specifies the desired position in degrees
 * e specifies elbow servo
 * --
 * v specifies the linear actuator
 * ### specifies the desired position, from 0 (fully retracted) to 255 (fully extended)
 * ex. v127; would tell the actuator to go to the halfway point
 * --
 * g specifies the gripper servo
 * ### specifies the desired position in degrees
 * ; specifies the end of the message
 * ex. g090; would tell the servo to go to 90 degrees
 */
void Manipulator::writeMicroAction(float s, float e, float v, float w) {
	// create serial obj

	CSerialStd ardu;
	//if (displayOn)
        printf("Try to write to microcontroller: ACTION\n");

	// open serial port
	assert( ardu.SetPort(USB_MICRO,115200) == CASRoboBus::KBusStatusOK);
	assert( ardu.Open() == CASRoboBus::KBusStatusOK);

    //if (displayOn)
        dPrint(1,"Positions sent: s = %f, e = %f, v = %f, w = %f\n",s*TODEG,e*TODEG,v,w*TODEG);

	Smove = s;
	Emove = e;
	Vmove = v;
	Wmove = w;

    //convert angles from floats to integers to decimal values of those integers
    //shoulder
	float sTemp = s*TODEG;
	int convert = (int) sTemp + 90; // add 90 to avoid minus signs. uC subs 90.
    int sends[3];
    sends[0]= (convert - (convert %100))/100 +48; // add 48 for '0' ASCII character number offset
	sends[1]= (convert %100 - convert%10)/10 +48;
	sends[2]= convert %10 +48;
    //elbow
	float eTemp = e*TODEG;
	convert = (int) eTemp + 115; // add 115 to avoid minus signs. uC subs 115.
    int sende[3];
    sende[0]= (convert - (convert %100))/100 +48;
	sende[1]= (convert %100 - convert%10)/10 +48;
	sende[2]= convert %10 +48;
    // vertical
	int vertical= (int) v;
	int sendv[3];
	sendv[0]= (vertical - (vertical %100))/100 +48;
	sendv[1]= (vertical %100 - vertical%10)/10 +48;
	sendv[2]= vertical %10 +48;
    // wrist
	float wTemp= w*TODEG;
	convert = (int) wTemp + 90;
    int sendw[3];
    sendw[0]= (convert - (convert %100))/100 +48;
	sendw[1]= (convert %100 - convert%10)/10 +48;
	sendw[2]= convert %10 +48;

    // combine all number arrays into one large char array
	char data[19];
	data[0] = (char) '$';
	data[1] = (char) 'A';
	data[2] = (char) 's';//startbyte shoulder
	data[3] = (char) sends[0];
	data[4] = (char) sends[1];
	data[5] = (char) sends[2];
	data[6] = (char) 'e';//startbyte elbow
	data[7] = (char) sende[0];
	data[8] = (char) sende[1];
	data[9] = (char) sende[2];
	data[10] = (char) 'v';//start byte lA
	data[11] = (char) sendv[0];
	data[12] = (char) sendv[1];
	data[13] = (char) sendv[2];
	data[14] = (char) 'w';	// Start byte gripper
	data[15] = (char) sendw[0];// angle gripper
	data[16] = (char) sendw[1];
	data[17] = (char) sendw[2];
	data[18] = (char) ';';	// end byte

	assert( ardu.Write((unsigned char*)data,19) == 19);

	// handshake?
	assert( ardu.Close() == CASRoboBus::KBusStatusOK);
	//printf("Action sent and closed.\n");

}

void Manipulator::writeMicroGripper(float g) {
	// create serial obj
	CSerialStd ardu;
	if (displayOn)
	printf("Try to write to microcontroller: GRIPPER \n");

	// open serial port
	assert( ardu.SetPort(USB_MICRO,115200) == CASRoboBus::KBusStatusOK);
	assert( ardu.Open() == CASRoboBus::KBusStatusOK);

	Gmove = g;

    //gripper
	int convert = (int) g;
    int sendg[3];
    sendg[0]= (convert - (convert %100))/100 +48; // add 48 for '0' ASCII character number offset
	sendg[1]= (convert %100 - convert%10)/10 +48;
	sendg[2]= convert %10 +48;


    // combine all number arrays into one large char array
	char data[7];

	data[0] = (char) '$';
	data[1] = (char) 'G';
	data[2] = (char) 'g';//startbyte shoulder
	data[3] = (char) sendg[0];
	data[4] = (char) sendg[1];
	data[5] = (char) sendg[2];
	data[6] = (char) ';';	// end byte

	assert( ardu.Write((unsigned char*)data,7) == 7);

	// handshake?
	assert( ardu.Close() == CASRoboBus::KBusStatusOK);
	//printf("Action sent and closed.\n");

}

void Manipulator::writeMicroGetState(){

        	// create serial obj
	CSerialStd ardu;
	if (displayOn)
	printf("Try to write to microcontroller: GET\n");

	// open serial port
	assert( ardu.SetPort(USB_MICRO,115200) == CASRoboBus::KBusStatusOK);
	assert( ardu.Open() == CASRoboBus::KBusStatusOK);

	char data[3];
	data[0] = (char) '$';
	data[1] = (char) 'M'; // M is all that matters here. The rest is just so that we don't have to change uC code
	data[2] = (char) ';';	// end byte

	assert( ardu.Write((unsigned char*)data,3) == 3);

	// handshake?
	assert( ardu.Close() == CASRoboBus::KBusStatusOK);
	//printf("Get sent and closed.\n");
}

void Manipulator::writeMicroReset(){
	// create serial obj
	CSerialStd ardu;
	if (displayOn)
	printf("Try to write to microcontroller: RESET\n");

	// open serial port
	assert( ardu.SetPort(USB_MICRO,115200) == CASRoboBus::KBusStatusOK);
	assert( ardu.Open() == CASRoboBus::KBusStatusOK);

	char data[5];
	data[0] = (char) '$';
	data[1] = (char) 'R'; // M is all that matters here. The rest is just so that we don't have to change uC code
	data[2] = (char) 'S';//startbyte shoulder
	data[3] = (char) 'T';
	data[4] = (char) ';';	// end byte

	assert( ardu.Write((unsigned char*)data,5) == 5);

	assert( ardu.Close() == CASRoboBus::KBusStatusOK);
	//printf("Get sent and closed.\n");
}

// read from microcontroller via serial communication from MaCI library
    Manipulator::EReadResult Manipulator::readMicro(char *message) {

    Manipulator::EReadResult done = RUNKNOWN;
	// create serial obj
	CSerialStd ardu;
	if (displayOn)
	dPrint(1,"Read from microcontroller:\n\n");

	// open serial port
	assert( ardu.SetPort(USB_MICRO,115200) == CASRoboBus::KBusStatusOK);
	assert( ardu.Open() == CASRoboBus::KBusStatusOK);

	// read from serial port
	// Note: serialtest.cpp uses ReadBlock, which does not appear in the header file...
	unsigned char buf[1000]; // buffer to read data to
	//for (int j=0 ; j <= 99 ; j++){ buf[j]= 'q';}
	//buf[14] = '\0';
	//assert( ardu.ReadBlock(buf,19,5000) == 19); // not sure what the 5000 is for


    //while(ardu.WaitToRead(50) != CASRoboBus::KBusStatusOK);

    int r = 0;
    int s = 1;
    while(s > 0 ){
        s = ardu.Read(buf + r, 3000);
        r +=s;
    }

    int messageLength = 22;

	// print out what was read and convert
	if (displayOn)
	fprintf(stderr, "\nRead %d bytes: ",r);
	for (int i=0; i<=r; i++) {
	    if (displayOn)
		fprintf(stderr, "%c", buf[i]);
	}
	if (displayOn)
	fprintf(stderr,"\n");

    for (int i=0; i<=r-1; i++){

        //update?
        if(buf[i]== '$' && buf[i+1]== 'U'){
            done = RUPDATE;
            //fprintf(stderr,"\n");
            for (int j=0; j<=messageLength; j++) {
            //fprintf(stderr, "%c", buf[i+j]);
            message[j]=buf[i+j];
            }
        }

        //done?
        if(buf[i]== '$' && buf[i+1]== 'D' && buf[i+2] == ';'){
            //fprintf(stderr,"\n");
            done = RDONE;
        }
    }

    return done;

}

/******************************************
*return 0 = Unknown case
*       1 = successfully executed
*       2 = no changes
*       3 = limits exdeeded
*/
 int Manipulator::actionExecuted(){
    if (!exceedLimitsFlag){
     switch (stateChangeFlag) {
         case 0:{return 0;}

         case 1:{
             char check[23];
             if (readMicro(check) == RDONE) return 1;
             else return 0;
         }
         case 2:{return 2;}
     }
    }else return 3;

    return 0;
}
/*
char Manipulator::convertangle(float in) {

	int degree= (int) in*TODEG;
	char convert = char(degree);
	return convert;
}*/
    void Manipulator::updateState(bool disp){

		if (disp) {
		dPrint(1,"states before update: ",X);
        dPrint(1,"x = %f ",X);
        dPrint(1,"y = %f ",Y);
        dPrint(1,"z = %f ",Z);
        dPrint(1,"c = %f ",C*TODEG);
        dPrint(1,"s = %f ",S*TODEG);
        dPrint(1,"e = %f ",E*TODEG);
        dPrint(1,"v = %f ",V);
        dPrint(1,"w = %f ",W*TODEG);
        dPrint(1,"g = %f ",G);
		}
        int i=0; // flag and counter
        do {
            writeMicroGetState();
            char updatedstate[23];
            //fprintf(stderr, "reveived status update:");

            if (readMicro(updatedstate) == RUPDATE){
                for (int j=0; j<=22; j++) {
                    if (disp)
                        fprintf(stderr, "%c", updatedstate[j]);
                }

            // note: issue with directly updating state directly from readMicro. Should be checked first?


            float sTemp = TORAD*(((((int) updatedstate[3])-48)*100 + (((int) updatedstate[4])-48)*10 + (((int) updatedstate[5])-48))-90);
            float eTemp = TORAD*(((((int) updatedstate[7])-48)*100 + (((int) updatedstate[8])-48)*10 + (((int) updatedstate[9])-48))-115);
            float vTemp = (((int) updatedstate[11])-48)*100 + (((int) updatedstate[12])-48)*10 + (((int) updatedstate[13])-48);
            float wTemp = TORAD*(((((int) updatedstate[15])-48)*100 + (((int) updatedstate[16])-48)*10 + (((int) updatedstate[17])-48))-90);
            int gTemp = (((int) updatedstate[19])-48)*100 + (((int) updatedstate[20])-48)*10 + (((int) updatedstate[21])-48);

            //measurements are inaccurate so we accept 5 degrees tolerance to prevent tiny movements or offsets
            float *poscheck=fwdKinematics(sTemp,eTemp,vTemp,wTemp);

            if (checkLimits(poscheck[0],poscheck[1],poscheck[2],poscheck[4],sTemp,eTemp,vTemp,wTemp,gTemp,disp)){

                // check that measurements fit within margins (the accuracy of the actuators) - checking is same as in the SET function
                //float sTemp = S; float eTemp = E; float vTemp = V; float wTemp = W; int gTemp = G;
                float anglemargin = 2.0*TORAD; // rad
                float margin = 2.0; // [mm]
                int gripAngleMargin = 5; // [deg]

				// update old state
				S_OLD = S;
				E_OLD = E;
				W_OLD = W;
				V_OLD = V;

				// update present state // outside previous position and inside expected position
                if (sTemp < S-anglemargin || sTemp > S+anglemargin) //&& (sTemp > Smove-anglemargin && sTemp < Smove+anglemargin))
                    S = sTemp;
                if (eTemp < E-anglemargin || eTemp > E+anglemargin) //&& (eTemp > Emove-anglemargin && eTemp < Emove+anglemargin))
                    E = eTemp;
                if (wTemp < W-anglemargin || wTemp > W+anglemargin) //&& (wTemp > Wmove-anglemargin && wTemp < Wmove+anglemargin))
                    W = wTemp;
                if (vTemp < V-margin || vTemp > V+margin) // add Vmove?
                    V = vTemp;
                if  (gTemp < G-gripAngleMargin || gTemp> G+gripAngleMargin) //&& (gTemp > Gmove-gripAngleMargin && gTemp < Gmove+gripAngleMargin))
                    G = gTemp;

                float *pos=fwdKinematics(S,E,V,W);
                X=pos[0];
                Y=pos[1];
                Z=pos[2];
                C=pos[3];


				if (disp) {
                dPrint(1,"states after update: \n",X);
                dPrint(1,"x = %f",X);
                dPrint(1,"y = %f",Y);
                dPrint(1,"z = %f",Z);
                dPrint(1,"c = %f",C*TODEG);
                dPrint(1,"s = %f",S*TODEG);
                dPrint(1,"e = %f",E*TODEG);
                dPrint(1,"v = %f",V);
                dPrint(1,"w = %f",W*TODEG);
                dPrint(1,"g = %f",G);
				}
                i=0; // indicate we can leave do/while loop, since we have an update

            } else if (i >= 2){
                dPrint(1,"second update still has wrong measurements");
                break;
            } else {
                i++;
                dPrint(1,"measurements out of bound: trying again");
            }
         } else i=1;
        } while ( i > 0 );

    }
